# -*- coding: utf-8 -*-
"""
Created on Fri Sep 23 09:11:23 2022
@author: LSY

Resilient sensor placement by adding one-by-one
"""
import numpy as np
from Sens_proj_cc import SensAnal

Nx = 88
Ny = 23
m = 23 # total avaliable sensor
r = 15 # selected sensor
Nsim = 10
rank_xp = True
sigma_w = 0
sigma_v = 0.001

M_all = list(range(Ny))
M_opt = [0,3,5,6,7,8,9,12,15,16,17,18,19,20,22]
N_known = 15
N_all = 23

N_add = 2  # The number of adding sensors

sm_all = np.loadtxt('sm_all.txt')

Sen_deg_opt = np.zeros((1,1));
deg_all_1 = np.zeros((N_all,N_all));
deg_all_2 = np.zeros((N_all,N_all));
for k in range(N_add):
    print("*********************************************************************")
    # Compute the current sensitive degree
    
    REs = open("Robust Results.txt", "a+")
    TT = np.eye(Nsim*(23))
    t_list = []  
    for ni in range(Nsim):        
        for nj in M_opt:
            t_list.append(int(nj+ni*(23))) 
    SM_I = sm_all[t_list,:]
    Flag = SensAnal(SM_I,Nx,N_known,sigma_w,sigma_v,Nsim-1,rank_xp) 
    Sen_deg_opt[:] = Flag[-1]
    print("Sen degree:", Sen_deg_opt)
    print("Sen degree:", Sen_deg_opt, file = REs)
    ###################################
    
    print(k+1,"- th add:")
    print(k+1,"- th add:", file = REs)
#    M_del = list(range(Ny));
#    for ii in range(N_known):
#        M_del.remove(M_opt[ii])
    Sen_deg_worst_opt = np.zeros((1,1))
    Robust_add_opt = np.zeros((1,1))
    Robust_rank_worst = np.zeros((1,1))
    for i in M_all:   # M_del
        print("-----------------------------------------------------------------------")
        print("-----------------------------------------------------------------------", file = REs)
        Sen_deg_worst = np.zeros((1,1))
        Sen_deg_worst[:] = Sen_deg_opt
        Sen_deg = np.zeros((1,1))
        Madd_try = np.concatenate((M_opt,i),axis = None) 
        Rank_worst = np.zeros((1,1))
        
        
        Worst_remove = np.zeros((1,1))
        for j in range(N_known+1): 
            print("--------------------------------")
            print('Try to add', i, 'and remove', Madd_try[j])
            print("--------------------------------", file = REs)
            print('Try to add', i, 'and remove', Madd_try[j], file = REs)
            Madd_try_ = np.delete(Madd_try,j,axis=0)
            print(Madd_try_)
            print(Madd_try_, file = REs)
            # calculate sensitivity matrix when remove j from set
            TT = np.eye(Nsim*(23)) 
            t_list = []       
            for ni in range(Nsim):        
                for nj in Madd_try_:
                    t_list.append(int(nj+ni*(23)))     
            SM_I = sm_all[t_list,:]
            Flag = SensAnal(SM_I,Nx,N_known,sigma_w,sigma_v,Nsim-1,rank_xp)

            Rank = Flag[0]
            Sen_deg[:] = Flag[-1]
            if k == 0:
                deg_all_1 [i,j] = Flag[-1]
            else:
                deg_all_2 [i,j] = Flag[-1]
            print("Rank", Rank)
            print("Rank", Rank, file = REs)
            print("Sen degree:", Sen_deg)
            print("Sen degree:", Sen_deg, file = REs)
            if Sen_deg <= Sen_deg_worst:
                Worst_remove[:] = Madd_try[j]
                Sen_deg_worst [:] = Sen_deg
                Rank_worst [:] = Rank
        print("Try to add", i, "     Worst of remove", Worst_remove) 
        print("Try to add", i, "     Worst of remove", Worst_remove, file = REs)
        print("Min Max degree:", Sen_deg_worst)
        print("Min Max degree:", Sen_deg_worst, file = REs)
        print("worst Rank:", Rank_worst)
        print("worst Rank:", Rank_worst, file = REs) 

        #From Sen_deg_worst select the best       
        if Sen_deg_worst >= Sen_deg_worst_opt:
           Sen_deg_worst_opt[:] = Sen_deg_worst
           Robust_add_opt[:] = i
           Robust_rank_worst[:] = Rank_worst
       
    N_known = N_known + 1  
    M_opt = np.concatenate((M_opt,Robust_add_opt),axis = None)
    print("+++++++++++++++++++++++++++++++++++++++++++++++++++")
    print("new added:", Robust_add_opt)
    print("new sensors:", M_opt)
    print("Sen degree:", Sen_deg_worst_opt)
    print("Rank:",Robust_rank_worst)
    print("+++++++++++++++++++++++++++++++++++++++++++++++++++", file = REs)
    print("new added:", Robust_add_opt, file = REs)
    print("new sensors:", M_opt, file = REs)
    print("Sen degree:", Sen_deg_worst_opt, file = REs)
    print("Rank:",Robust_rank_worst, file = REs)
    REs.close()


np.savetxt("deg_all_1.txt", deg_all_1)      
np.savetxt("deg_all_2.txt", deg_all_2) 